/**
 * Implement the hierarchy mesh. 
 * @author Songgang Xu (sxu at tamu dot edu)
 * @file model.h
 */


#ifndef _MESHCOLOR_GCMESH_H
#define _MESHCOLOR_GCMESH_H
#include <set>
#include <string>

#include "primitive.h"
#include "predicate_3d.h"
#include "intersect_3d.h"

#include "mesh.h"
#include "aabbtree.h"


namespace math 
{
	template <typename value_, unsigned dim> class HierarchyMeshFace ;
	template <typename value_, unsigned dim> class HierarchyMeshVertex ;
	template <typename Value , unsigned dim> struct hierarchy_mesh_traits {} ;
	template <typename value> struct hierarchy_mesh_traits <value, 3> : public mesh_traits <value, 3> 
	{
		typedef HierarchyMeshFace <value,3 > FaceType ;
		typedef HierarchyMeshVertex <value,3 > VertexType ;
		typedef HalfEdge <value,3 > HalfEdgeType ;
	} ;

	template <typename value_, unsigned dim>
		class HierarchyMeshVertex : public Vertex <value_, dim>
	{
	public :
		typedef value_ value_type ;
		typedef typename math::vector<value_type, 3> point_type ;
		typedef point_type vector_type ; 
		typedef typename Vertex <value_type, dim>::VFIter VFIter ;

	    HierarchyMeshVertex() : locked(false ) , Vertex<value_, dim> () {}
		bool locked ; 

		vector_type m_vertex_normal; 
		void compute_vertex_normal ()
		{
			vector_type n (0.0f, 0.0f, 0.0f);
			VFIter vfiter = this->vfbegin_safe();
			do 
			{
				n = n + (*vfiter)->normal();
				++ vfiter;
			} while (vfiter != this->vfend_safe());
			this->m_vertex_normal = n.normalize(); 			
		}
		void construct ()
		{
			compute_vertex_normal();
		}
	}; 

	template <typename value_, unsigned dim >
	class Layer
	{
	public: 
		typedef value_ value_type; 
		typedef typename math::vector<value_type, 3> point_type ;
		typedef point_type vector_type ; 
		typedef math::vector<float, 4> frgba_type ;
	public:
		friend std::istream & operator >> (std::istream & istr, Layer & obj)
		{
			char tag ;
			istr >> tag ;
			if (tag != 'r') throw ("Wrong layer tag.");
			istr >> obj.resolution ; 
			for (unsigned i = 0; i < (obj.resolution +1)  *(obj.resolution+2)/2; ++i)
			{
				frgba_type color ;
				istr >> color.r >> color.g >> color.b >> color.a ;
				obj.pixel[i] = color ;
			}
			std::string line; 
			getline (istr, line);
			return istr ;
		}
		friend std::ostream & operator << (std::ostream & ostr, const Layer & obj)
		{
			ostr << 'r'<< ' '<<obj.resolution <<std::endl;
			for (unsigned i = 0; i < (obj.resolution +1)  *(obj.resolution+2)/2; ++i)
			{
				ostr <<obj.pixel[i].r 
					 <<' '<<obj.pixel[i].g
					 <<' '<<obj.pixel[i].b
					 <<' '<<obj.pixel[i].a<<' ';
			}
			ostr<<std::endl;
			return ostr ; 
		} 
	public:
		/*
        Layer () : resolution (16) 
        {
			pixel.resize ( (resolution +1)  *(resolution+2)/2);
        }
		*/
        Layer (unsigned short r, frgba_type default_color): resolution (r) 
        {
			pixel.resize ( (resolution +1)  *(resolution+2)/2);
			for (int i = 0; i < (resolution +1)  *(resolution+2)/2; ++i)
			{
				pixel[i]= default_color ;
			}
        }

		int barycentric2index (unsigned res,  unsigned i, unsigned j)
		{
			return  (res + 1 + res + 2 - i ) * i / 2 + j; 
		}
		frgba_type  linear_blend (const frgba_type & front_color, const frgba_type & back_color)
        {
			return  (front_color * 0.5f + back_color * 0.5f) ;
        }
        void upSampling () 
        {
			std::vector<frgba_type> old_pixel (pixel);
			unsigned short res = this->resolution ;
			res *= 2 ;
			this->pixel.resize( (res+1) * (res+2) / 2);
			this->resolution = res; 

			for (unsigned i = 0; i <=res; ++i)
			{
				for (unsigned j = 0; j <= res - i ; ++j)
				{
					int k = res - i - j;
					if (i % 2 == 0 && j % 2 == 0 && k % 2 == 0)
					{
						pixel[barycentric2index(res, i,j)] =
							old_pixel[barycentric2index(res/2, i/2,j/2)] ;
						continue;
					}
					//Fix i, two neighbors are (i,j-1,k+1) and (i, j+1,k-1)
					if (i%2 == 0)
					{
						//pixel[barycentric2index(res, i, j)] =
						//      old_pixel[barycentric2index(res/2, i/2,(j-1)/2)] * 0.5f 
						//      + old_pixel[barycentric2index(res/2, i/2,(j+1)/2)] * 0.5f;
						pixel[barycentric2index(res, i, j)] =
							linear_blend(old_pixel[barycentric2index(res/2, i/2,(j-1)/2)] ,
										 old_pixel[barycentric2index(res/2, i/2,(j+1)/2)]);
						continue;
					}
					//Fix j, two neighbors are (i-1,j,k+1) and (i+1, j,k-1)
					if (j%2 == 0)
					{
						//pixel[barycentric2index(res, i, j)] =
						//      old_pixel[barycentric2index(res/2, (i-1)/2,(j)/2)] * 0.5f 
						//      + old_pixel[barycentric2index(res/2, (i+1)/2,(j)/2)] * 0.5f;

						pixel[barycentric2index(res, i, j)] =
							linear_blend(   old_pixel[barycentric2index(res/2, (i-1)/2,(j)/2)]
											, old_pixel[barycentric2index(res/2, (i+1)/2,(j)/2)] );
						continue;
					}
					//Fix k, two neighbors are (i-1,j+1,k) and (i+1, j-1,k)
					if (k%2 == 0)
					{
						//pixel[barycentric2index(res, i, j)] =
						//      old_pixel[barycentric2index(res/2, (i+1)/2,(j-1)/2)] * 0.5f 
						//      + old_pixel[barycentric2index(res/2, (i-1)/2,(j+1)/2)] * 0.5f;
						pixel[barycentric2index(res, i, j)] =
							linear_blend( old_pixel[barycentric2index(res/2, (i+1)/2,(j-1)/2)] , 
										  old_pixel[barycentric2index(res/2, (i-1)/2,(j+1)/2)] );
                                        
						continue;
					}
				}
			}
        }
        void downSampling () 
        {
			unsigned short res = this->resolution ;
			res /= 2;
			for (unsigned i = 0 ; i <= res; ++i)
			{
				for (unsigned j = 0; j <= res - i; ++j)
				{
					this->pixel[barycentric2index(res, i,j)] = 
						this->pixel [barycentric2index (res * 2, i *2, j * 2)] ;
				}
			}
			this->resolution = res; 
			this->pixel.resize((res+1) * (res+2) / 2);
        }
        inline frgba_type & operator [](unsigned i)  
        {
			return pixel [i] ;
        }

        inline const unsigned short getResolution() const 
        {
			return this->resolution ;
        }
	protected:
        unsigned short resolution; 
        std::vector<frgba_type> pixel ;
	};

	template <typename value_, unsigned dim>
		class HierarchyMeshFace : public Face <value_, dim>
	{
	public :
		typedef value_ value_type; 
		typedef HierarchyMeshFace <value_type, dim> face_type ;
		typedef typename Face <value_type, dim>::PointType point_type ;
		typedef point_type vector_type ; 
		typedef typename Face <value_type, dim>::FVIter FVIter ;
		typedef math::vector<float, 4> frgba_type ;
		typedef Layer <value_type, 3> layer_type ;
		typedef typename hierarchy_mesh_traits<value_, dim>::HalfEdgeType halfedge_type ;
		typedef typename hierarchy_mesh_traits<value_, dim>::VertexType vertex_type ;

	public :
		friend std::istream & operator >> (std::istream & istr, face_type & obj)
		{
			char tag ;
			unsigned start_vertex_idx, layer_size ; 
			std::string line; 
			istr >> tag ; 
			if (tag != 's') 
			{
				std::cout<<"tag "<<line<<std::endl;
				istr.setstate(std::ios_base::eofbit) ;
				return istr ;
				throw ("Wrong face tag.");
			}
			istr >> start_vertex_idx ;
			// adjust the face he pointer 
			for (FVIter iter = obj.fv_begin(); iter != obj.fv_end(); ++iter)
			{
				if (iter.get_he()->from_v->m_idx == start_vertex_idx)
				{
					obj.he = iter.get_he(); 
				}
			}


			getline (istr, line);
			istr >> tag ;
			if (tag != 'l') throw ("Wrong face layer tag.");
			istr >> layer_size ;
			getline (istr, line);
			
			for (unsigned i = 1; i < layer_size + 1 ;++i)
			{
				istr >> (*obj.m_layers[i]); 
			}
			return istr ;
		}
		friend std::ostream & operator << (std::ostream & ostr, const face_type  & obj)
		{
			ostr <<"s "<<(obj.he->from_v->m_idx) <<std::endl;
			ostr <<"l "<< obj.m_layers.size() - 1<<std::endl; 
			for (unsigned i = 1; i < obj.m_layers.size()  ; ++i)
			{
				ostr << (*obj.m_layers[i]) ;
			}
			return ostr ; 
		} 
	public :
		~ HierarchyMeshFace () 
		{
			for (unsigned i = 0; i < m_layers.size(); ++i)
			{
				delete m_layers[i]; 
			}
		}
		unsigned get_resolution() const {return this->m_res ;}
		unsigned get_num_samples () const {return (m_res + 1 ) * (m_res + 2) / 2 ; }
		void set_memid (unsigned i ) {this->m_memid = i; }
		void set_texture (Texture * tm) {this->ptm = tm ;}
		bool is_selected () const {return this->m_selected; }
		bool set_selected (bool flag = true) {this->m_selected = flag; }

		frgba_type blend_color (const frgba_type ca, const frgba_type cb)
		{
			// ca is over cb ;
			frgba_type co ;
			co.a = 1.0f - (1.0f- ca.a) * (1.0f-cb.a) ;

			co.r = (ca.a * ca.r + (1.0f - ca.a) * cb.a * cb.r ) ;
			co.g = (ca.a * ca.g + (1.0f - ca.a) * cb.a * cb.g ) ;
			co.b = (ca.a * ca.b + (1.0f - ca.a) * cb.a * cb.b ) ;
			// co.a singularity. 
			if (std::abs((co.a)) > 1e-5)
			{
				co.r /= co.a; co.g /=co.a; co.b /= co.a; 
			}
			return co ;
		}

		frgba_type blend_layer_color (unsigned layer_idx, unsigned idx)
		{
			frgba_type canvas = (*m_layers[0])[idx] ;
			frgba_type color = (*this->m_layers[layer_idx])[idx] ; 
			return blend_color (color, canvas);
		}

		
		frgba_type blend_all_layer_color (unsigned idx)
		{
			frgba_type canvas = (*m_layers[0])[idx] ;
		
			for (unsigned i = 1 ; i < m_layers.size(); ++i)
			{
				frgba_type color = (*this->m_layers[i])[idx] ; 
				canvas = blend_color (color, canvas);
			}
			return canvas ;
		}
		
		/*
		frgba_type blend_all_layer_color (unsigned idx)
		{
			frgba_type c0 = m_layers[0][idx];
			frgba_type c1 = m_layers[1][idx];
			frgba_type c2 = m_layers[2][idx];
			//return blend_color (c2, blend_color(c1, c0));
			return blend_color (blend_color(c2, c1), c0) ;
		}
		*/
		void set_color (unsigned layer_idx, unsigned idx, const frgba_type & color)
		{
			
			(*this->m_layers[layer_idx])[idx] = color ;

			if (m_merge_all_layers == false)
			{
				ptm->update (m_memid, idx + 4, blend_layer_color(layer_idx, idx)) ;
			}
			else ptm->update (m_memid, idx + 4, blend_all_layer_color(idx)) ;
		}
		void set_blend_color (unsigned layer_idx, unsigned idx, const frgba_type & color)
		{
			frgba_type old_color = (*this->m_layers[layer_idx])[idx]; 
			(*this->m_layers[layer_idx])[idx] = blend_color (color, old_color);

			if (m_merge_all_layers == false)
			{
				ptm->update (m_memid, idx + 4, blend_layer_color(layer_idx, idx)) ;
			}
			else ptm->update (m_memid, idx + 4, blend_all_layer_color(idx)) ;
		}

		virtual void construct ()
		{
			this->m_res = 32 ;
			this->m_layer_id = 1; 
			this->m_selected = false ;
			this->m_tagged = false; 
			this->m_merge_all_layers = false ;
			
			adjust_obtuse(); // tweak the obtuse 
			std::vector<point_type * >  v_set ;
			this->Face2VertexCoord (v_set ); 
			point[0] =  * v_set[0];
			point[1] =  * v_set[1];
			point[2] =  * v_set[2];
			compute_normalized_tex_param () ;
			this->compute_normal(); 

			// Canvas ;
			layer_type * layer = new layer_type(m_res, frgba_type(1.0, 1.0, 1.0, 1.0)) ;  
			this->m_layers.push_back(layer);
			layer_type * layer1 = new layer_type(m_res, frgba_type(0.0, 0.0, 0.0, 0.0)) ; 
			layer_type * layer2 = new layer_type(m_res, frgba_type(0.0, 0.0, 0.0, 0.0)) ; 
			this->m_layers.push_back(layer1);    // Layer 1 ; 
			this->m_layers.push_back(layer2);    // Layer 2 ; 
			for (unsigned i = 0; i < 3; ++i) this->m_strong_edge[i] = false; 
		}
		

		void set_layer_id (unsigned id ) {this->m_layer_id = id ;}
		void paint_face_texture ()
		{
			unsigned row = m_memid ;
			unsigned col = 0; 
			TexCoord texcoord ; 
			texcoord.x = col + 0.1f;  // Padding
			texcoord.y = row + 0.1f;  // Padding
			ptm->update (row, 0, frgba_type (m_res, 0.0f, 0.0f, 0.0f));

			if (k>=0)
			{
				ptm->update(row, 1, frgba_type (texcoord.x, texcoord.y, 0.0f, 0.0f));
				ptm->update (row, 2, frgba_type (texcoord.x + c, texcoord.y , 0.0f, 0.0f));
				ptm->update (row, 3, frgba_type (texcoord.x + k, texcoord.y + h, 0.0f, 0.0f));
			}
			else if (k < 0)
			{
				std::cout<<"minus "<<std::endl; 
				ptm->update(row, 1, frgba_type (texcoord.x - k, texcoord.y, 0.0f, 0.0f)); 
				ptm->update(row, 2, frgba_type (texcoord.x - k + c, texcoord.y, 0.0f, 0.0f )); 
				ptm->update(row, 3, frgba_type (texcoord.x, texcoord.y + h, 0.0f, 0.0f)); 
			}

			for (unsigned i = 0; i < (m_res + 1 ) * (m_res + 2) / 2 ; ++i ) 
			{

				/*
				if (i % 3 == 1)
					ptm->update (row, i, frgba_type (1.0f, 0.0f, 0.0f, 1.0f));
				else if (i %3 == 2)
					ptm->update (row, i, frgba_type (0.0f, 1.0f, 0.0f, 1.0f));
				else 
					ptm->update (row, i, frgba_type (0.0f, 0.0f, 1.0f, 1.0f));
				*/
				frgba_type color = (*m_layers[m_layer_id])[i] ;
				set_color (m_layer_id, i, color); 
			}
		}

		
		void draw_tex_blend() 
		{
			glDisable(GL_LIGHTING); 
			glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
			glEnable(GL_BLEND);
			glColor4f(0.1,0.75,0.75, 0.5);
			glBegin(GL_TRIANGLES);
			std::vector < vector<float, 2>  * > v_texset; 
			this->Face2TexCoord(v_texset ); 
			vector<float, 2> tex[3] ;
			tex[0] = *v_texset[0]; 			tex[1] = *v_texset[1];  			tex[2] = *v_texset[2];

			point_type normal = vector_type(0, 0, 1);
			glNormal3f(normal.x, normal.y, normal.z); 
//			point_type p0 = point[0] + point_type (5, 5, 5);
			glVertex3f(tex[0].x, tex[0].y, 0.0f);
			glVertex3f(tex[1].x, tex[1].y, 0.0f);
			glVertex3f(tex[2].x, tex[2].y, 0.0f);
			glEnd();
			glDisable(GL_BLEND);
			glEnable(GL_LIGHTING); 
		}

		void draw_blend() 
		{
			glDisable(GL_LIGHTING); 
			glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
			glEnable(GL_BLEND);
			glColor4f(0.1,0.75,0.75, 0.5);
			glBegin(GL_TRIANGLES);
			point_type normal = this->get_normal ();
			glNormal3f(normal.x, normal.y, normal.z); 
//			point_type p0 = point[0] + point_type (5, 5, 5);
			glVertex3f(point[0].x, point[0].y, point[0].z);
			glVertex3f(point[1].x, point[1].y, point[1].z);
			glVertex3f(point[2].x, point[2].y, point[2].z);
			glEnd();
			glDisable(GL_BLEND);

		}
		void draw_texcoord() 
		{
			unsigned row = m_memid ;
			unsigned col = 0; 
			TexCoord texcoord ; 
			texcoord.x = col + 0.1f;  // Padding
			texcoord.y = row + 0.1f;  // Padding 

			std::vector < vector<float, 2>  * > v_texset; 
			this->Face2TexCoord(v_texset ); 
			vector<float, 2> tex[3] ;
			tex[0] = *v_texset[0]; 			tex[1] = *v_texset[1];  			tex[2] = *v_texset[2]; 
			glBegin(GL_TRIANGLES);
			point_type normal = vector_type (0, 0, 1);//this->get_normal ();
			glNormal3f (normal.x, normal.y , normal.z); 
//			if (k >=0 )
//			{
				glTexCoord2f(texcoord.x, 		texcoord.y); glVertex3f(tex[0].x, tex[0].y, 0.0f);
				glTexCoord2f(texcoord.x + c , 	texcoord.y ); glVertex3f(tex[1].x, tex[1].y, 0.0f);
				glTexCoord2f(texcoord.x + k , 	texcoord.y + h); glVertex3f(tex[2].x, tex[2].y, 0.0f);
//			}
//			else
//			{
//				glTexCoord2f(texcoord.x - k, 		texcoord.y); glVertex3f(tex[0].x, tex[0].y, 0.0f);
//				glTexCoord2f(texcoord.x - k + c , 	texcoord.y ); glVertex3f(tex[1].x, tex[1].y, 0.0f);
//				glTexCoord2f(texcoord.x , 	texcoord.y + h); glVertex3f(tex[2].x, tex[2].y, 0.0f);
//			}
			glEnd();		
			
		}

		void draw_smooth()
		{
			unsigned row = m_memid ;
			unsigned col = 0; 
			TexCoord texcoord ; 
			texcoord.x = col + 0.1f;  // Padding
			texcoord.y = row + 0.1f;  // Padding 
	
			glBegin(GL_TRIANGLES);
			point_type normal = this->get_normal ();
			glNormal3f (normal.x, normal.y , normal.z); 

			typedef typename mesh_traits<value_type, dim>::VertexType p_vertex_type; 
			std::vector<p_vertex_type *> v_vertex ;
			this->Face2Vertex(v_vertex);
//			if (k >=0 )
//			{
				vertex_type * v0 = (vertex_type *) v_vertex[0] ;
			    glNormal3f (v0->m_vertex_normal.x, v0->m_vertex_normal.y, v0->m_vertex_normal.z);
				glTexCoord2f(texcoord.x, 		texcoord.y); glVertex3f(point[0].x, point[0].y, point[0].z);
				vertex_type * v1 = (vertex_type *) v_vertex[1] ;
			    glNormal3f (v1->m_vertex_normal.x, v1->m_vertex_normal.y, v1->m_vertex_normal.z);

				glTexCoord2f(texcoord.x + c , 	texcoord.y ); glVertex3f(point[1].x, point[1].y, point[1].z);
				vertex_type * v2 = (vertex_type *) v_vertex[2] ;
			    glNormal3f (v2->m_vertex_normal.x, v2->m_vertex_normal.y, v2->m_vertex_normal.z);
				glTexCoord2f(texcoord.x + k , 	texcoord.y + h); glVertex3f(point[2].x, point[2].y, point[2].z);
//			}
//			else
//			{
//				glTexCoord2f(texcoord.x - k, 		texcoord.y); glVertex3f(point[0].x, point[0].y, point[0].z);
//				glTexCoord2f(texcoord.x - k + c , 	texcoord.y ); glVertex3f(point[1].x, point[1].y, point[1].z);
//				glTexCoord2f(texcoord.x , 	texcoord.y + h); glVertex3f(point[2].x, point[2].y, point[2].z);
//			}
			glEnd();	
		}

		void draw_flat()
		{
			unsigned row = m_memid ;
			unsigned col = 0; 
			TexCoord texcoord ; 
			texcoord.x = col + 0.1f;  // Padding
			texcoord.y = row + 0.1f;  // Padding 
	
			glBegin(GL_TRIANGLES);
			point_type normal = this->get_normal ();
			glNormal3f (normal.x, normal.y , normal.z); 
//			if (k >=0 )
//			{
				glTexCoord2f(texcoord.x, 		texcoord.y); glVertex3f(point[0].x, point[0].y, point[0].z);
				glTexCoord2f(texcoord.x + c , 	texcoord.y ); glVertex3f(point[1].x, point[1].y, point[1].z);
				glTexCoord2f(texcoord.x + k , 	texcoord.y + h); glVertex3f(point[2].x, point[2].y, point[2].z);
//			}
//			else
//			{
//				glTexCoord2f(texcoord.x - k, 		texcoord.y); glVertex3f(point[0].x, point[0].y, point[0].z);
//				glTexCoord2f(texcoord.x - k + c , 	texcoord.y ); glVertex3f(point[1].x, point[1].y, point[1].z);
//				glTexCoord2f(texcoord.x , 	texcoord.y + h); glVertex3f(point[2].x, point[2].y, point[2].z);
//			}
			glEnd();	
		}

        void draw_strong_edge ()
        {
			//draw strong edges ;
			std::vector<point_type * > p;
			Face2VertexCoord(p); 
			glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
			glEnable(GL_BLEND);
			glLineWidth(5);
			glDisable(GL_LIGHTING);
			glBegin(GL_LINES);
			vector_type normal = this->get_normal();
			normal.normalize(); 
			for (int i = 0; i < 3; ++i)
			{
				if (get_strong_edge(i))
				{
					point_type p1 = ( *p[i] ) + normal * hardedge_dis ;
					point_type p2 = ( *p[(i+1)%3]) + normal  * hardedge_dis;
					glVertex3f (p1.x, p1.y, p1.z);
					glVertex3f (p2.x, p2.y, p2.z);
					//glVertex3f((*p[i]).x, (*p[i]).y, (*p[i]).z);
					//glVertex3f((*p[(i+1)%3]).x, (*p[(i+1)%3]).y, (*p[(i+1)%3]).z);
				}
			}
			glEnd();
			glDisable(GL_BLEND);
			glEnable(GL_LIGHTING);
        }

		void up_sampling ()
		{
			if (this->m_res == 64) return ; 
			m_res *= 2 ; 
			for (unsigned i = 0; i < m_layers.size(); ++i)
				m_layers[i]->upSampling(); 
		}
		void down_sampling()
		{
			if (this->m_res == 1) return ;
			m_res /= 2 ;
			for (unsigned i = 0; i < m_layers.size(); ++i) 
				m_layers[i]->downSampling(); 
		}
	private :
		void adjust_obtuse ()
		{
			std::vector<point_type * >  v_set ;
			this->Face2VertexCoord (v_set ); 
			point_type p[3];
			p[0] =  * v_set[0];
			p[1] =  * v_set[1];
			p[2] =  * v_set[2];

			float a = distance(p[1], p[2]) ;
			float b = distance(p[2], p[0]) ;
			float c = distance(p[0], p[1]) ;

			halfedge_type * he0 = this->he; 
			halfedge_type * he1 = he0->next ;
			halfedge_type * he2 = he1->next ;
			if (a >=b && a >=c )
			{
				this->he = he1; 
				return ;
			}
			if (b >=c && b >=a) 
			{
				this->he = he2 ;
				return ;
			}
			this->he = he0 ;
		}
		GLfloat compute_k(point_type p[3])
		{
			float a = distance(p[1], p[2]) ;
			float b = distance(p[2], p[0]) ;
			float c = distance(p[0], p[1]) ;
			float k = (b*b + c * c - a * a) / (2.0 * c) ; 
			return GLfloat (k); 
		}
		GLfloat compute_h(point_type p[3])
		{
			float a = distance(p[1], p[2]) ;
			float b = distance(p[2], p[0]) ;
			float c = distance(p[0], p[1]) ;
			float cos_a = (b*b + c* c - a* a)/ (2.0 * b * c) ;
			float sin_a = sqrt (1.0 - cos_a * cos_a) ;
			return (b * sin_a) ;
		}
		GLfloat compute_c(point_type p[3])
		{
			return distance(p[0] , p[1]) ;
		}
		void compute_normalized_tex_param ()
		{
			/*
			k = compute_k(point);
			h = compute_h(point);
			c = compute_c(point);
			*/

			float a = distance(point[1], point[2]) ;
			float b = distance(point[2], point[0]) ;
			this->c = distance(point[0], point[1]) ;
			this->k = (b*b + c * c - a * a) / (2.0 * c) ; 
			float cos_a = (b*b + c* c - a* a)/ (2.0 * b * c) ;
			float sin_a = sqrt (1.0 - cos_a * cos_a) ;
			this->h = (b * sin_a) ;
			

			if (h >=c) { k /= h ; c/=h ; h = 1.0f ; }
			else{ k /= c; h /= c; c = 1.0; }
			if (k < 0)
			{
				float ratio = c - k ;
				k /= ratio ; c /= ratio; h /= ratio ; 
			}
			if (k > 1.0)
			{
				float ratio = c + k ;
				k /= ratio ; c /=ratio ; h /=ratio ;
			}
			k *= 0.8; c *=0.8; h *=0.8; 
		}
	public :
		halfedge_type * get_he_by_counter (unsigned count)
        {
                halfedge_type * result = this->he; 
                for (unsigned i = 0; i < count ; ++i) 
                        result = result->next ;
                return result ;
        }
		unsigned get_halfedge_counter (halfedge_type * he)
		{
			halfedge_type * result = this->he ;
			unsigned i = 0 ; 
			for (i = 0; i < 3; ++i)
			{
				if (he == result) return i ;
				result = result->next ;
			}
			return i; 
		}
		void set_tagged (bool flag = true) {this->m_tagged = flag; }
		bool get_tagged () const {return this->m_tagged; }
		bool get_merge_all_layers () const {return m_merge_all_layers ;}
		void set_merge_all_layers (bool flag = true){this->m_merge_all_layers = flag ;}
		bool flip_strong_edge(unsigned i) {this->m_strong_edge[i] = !this->m_strong_edge[i]; }
		bool set_strong_edge (unsigned i, bool flag = true) {this->m_strong_edge[i] = flag; }
		bool get_strong_edge (unsigned i) const {return this->m_strong_edge[i] ; }
	private:
		static constexpr value_type hardedge_dis = 0.01f;
		unsigned m_memid ;
		float k, h, c ;
		Texture * ptm ;
		unsigned m_res;
		unsigned m_layer_id ;
		point_type point[3];
		bool m_selected ;

		bool m_tagged ;

		std::vector<layer_type* > m_layers ;
		bool m_merge_all_layers ;

		bool m_strong_edge[3];
	public :


	};

	template<typename mesh_traits>
		class HierarchyMesh : public Mesh <mesh_traits>
	{
	public:
	   	typedef typename mesh_traits::ValueType              ValueType; 
		typedef typename mesh_traits::FaceType               FaceType;
		typedef typename mesh_traits::VertexType             VertexType;
		typedef typename mesh_traits::HalfEdgeType           HalfEdgeType ; 
		typedef typename mesh_traits::EdgeType               EdgeType ; 
		typedef typename mesh_traits::VectorType             VectorType ;
		typedef typename mesh_traits::PointType              PointType ;
//		typedef typename mesh_traits::MeshType               MeshType ;
		typedef typename Mesh<mesh_traits>::HalfEdgeIterator HalfEdgeIterator ;
		typedef typename Mesh<mesh_traits>::VertexIterator   VertexIterator ;
		typedef Mesh<mesh_traits>                            parent_type ;
		typedef Segment<ValueType, mesh_traits::dim>         SegmentType ;
		typedef Ray<ValueType, mesh_traits::dim>             RayType ;

		typedef AABBTreePoly <ValueType, mesh_traits::dim>          AABBTreePolyType; 
		typedef AABBTreePolyBuilder <ValueType, mesh_traits::dim>   AABBTreePolyBuilderType  ; 
		typedef AABBTreePolygon <ValueType, mesh_traits::dim>       AABBTreePolygonType ;
		typedef AABBTreeNode <ValueType, mesh_traits::dim>          AABBTreeNodeType ;

		typedef Box<ValueType, mesh_traits::dim>                    BoxType; 
		typedef Polygon<ValueType, mesh_traits::dim>                PolygonType; 
		typedef Sphere<ValueType, mesh_traits::dim>                 SphereType; 

		typedef typename AABBTreePolyType::PolygonColResult         PolygonColResult;
		typedef typename AABBTreePolyType::RayColResult             RayColResult;
		typedef typename AABBTreePolyType::SphereColResult          SphereColResult;
		typedef typename AABBTreePolyType::SegmentColResult         SegmentColResult;

	public: 
	    HierarchyMesh(): mp_tree( NULL ) {}
		virtual ~HierarchyMesh() 
		{
			if (this->mp_tree != NULL)
				delete this->mp_tree; 
		}
///////////////////Loader Writer///////////////////
		virtual std::istream & OBJ_reader (std::istream & istr)
		{
			parent_type::OBJ_reader (istr); 
			
			std::string line; 
//			getline(istr, line);
			for (unsigned i = 0; i < this->faces.size(); ++i)
			{
				if ( !(istr >> (*this->faces[i]) )) break; 
			}
			
			return istr ;
		}
		virtual std::ostream & OBJ_writer (std::ostream & ostr)
		{
			parent_type::OBJ_writer (ostr); 
			ostr<<"meshcolor"<<std::endl;
			for (unsigned i = 0; i < this->faces.size(); ++i)
			{
				ostr << (*this->faces[i]) ; 
			}
			return ostr ;
		}
//		template <typename mesh_traits_type>
		AABBTreePolyType * build_hierarchy ()
		{
			if (this->mp_tree != NULL) delete this->mp_tree; 
			AABBTreePolyBuilderType builder ;
//			this->mp_tree = builder.build_from_mesh ( (Mesh<mesh_traits > &) (* this), 1);
			this->mp_tree = builder.build_from_mesh ( (* this), 1);
			return this->mp_tree; 
		}
		AABBTreePolyType * get_aabbtree() const 
		{
			return this->mp_tree; 
		}

		bool is_intersect (const PolygonType & polygon)
		{
			PolygonColResult result ;
			this->mp_tree->collideWithPolygon (polygon, result);
			return result.polys_.size() > 0; 
		}
		bool is_intersect (const SphereType & sphere)
		{
			SphereColResult result ;
			this->mp_tree->collideWithSphere (sphere, result);
			return result.polys_.size() > 0; 
		}
		bool intersect (const PolygonType & polygon, std::vector<unsigned > & v_face_id)
		{
			PolygonColResult result ;			
			this->mp_tree->collideWithPolygon (polygon, result);
			if ( result.polys_.size() == 0) return false; 
			for (unsigned i = 0; i < result.polys_.size(); ++i)
			{
				v_face_id.push_back(result.polys_[i]->get_face()->m_idx);
			} 
			return true; 
		}


		bool intersect (const SphereType & sphere, std::vector<unsigned > & v_face_id)
		{
			SphereColResult result ;			
			this->mp_tree->collideWithSphere (sphere, result);
			if ( result.polys_.size() == 0) return false; 
			for (unsigned i = 0; i < result.polys_.size(); ++i)
			{
				v_face_id.push_back(result.polys_[i]->get_face()->m_idx);
			} 
			return true; 
		}

		bool intersect (const SegmentType & seg, std::vector<unsigned > & v_face_id)
		{
			SegmentColResult result ;			
			this->mp_tree->collideWithSegment (seg, result);
			if ( result.polys_.size() == 0) return false; 
			for (unsigned i = 0; i < result.polys_.size(); ++i)
			{
				v_face_id.push_back(result.polys_[i]->get_face()->m_idx);
			} 
			return true; 
		}
		// We assume this segment is oriented. p0 --> p1. We pick the face nearest to p0. 
		bool pick (const SegmentType & seg, std::vector<unsigned > & v_face_id)
		{
			SegmentColResult result ;			
			this->mp_tree->SegmentPick (seg, result);
			if ( result.polys_.size() == 0) return false; 
			for (unsigned i = 0; i < result.polys_.size(); ++i)
			{
				v_face_id.push_back(result.polys_[i]->get_face()->m_idx);
			} 
			return true; 			
		}
	private :
		AABBTreePolyType * mp_tree; 
	};


	template <typename value_, unsigned dim>
	class PseudoMesh 
	{

	typedef float value_type ;
	typedef math::vector<value_type, 2> vector2 ;
	typedef math::vector<value_type, 3> vector3 ;
	typedef math::vector<value_type, 2> point2 ;
	typedef math::vector<value_type, 3> point3 ;

	typedef math::HierarchyMesh<math::hierarchy_mesh_traits<value_type, 3> >  mesh_type ;
	typedef math::hierarchy_mesh_traits <value_type, 3>::FaceType face_type ;
	public:
        PseudoMesh (mesh_type * mesh): m_pmesh (mesh)
        {

        }
	public:
        inline unsigned pseudo2mesh_idx (unsigned pseudo_idx) const
        {
			return idx_pseudo2mesh[pseudo_idx];
        }
        inline unsigned mesh2pseudo_idx (unsigned mesh_idx) const
        {
			return idx_mesh2pseudo[mesh_idx];
        }

	public: 
        inline mesh_type * getMesh () {return this->m_pmesh;}
        inline void  Face2MeshVertexIdx (face_type * face, std::vector<int> & idx) 
        {
			//mesh->Face2VertexIdx(face, idx );
			
			face->Face2VertexIdx(idx);
			idx[0] = pseudo2mesh_idx(idx[0]);
			idx[1] = pseudo2mesh_idx(idx[1]);
			idx[2] = pseudo2mesh_idx(idx[2]);
        }

		// maybe cost too much memory. 
        void construct_vertex_mapping ()
        {
			this->idx_mesh2pseudo.clear(); 
			this->idx_pseudo2mesh.clear(); 
                std::set < unsigned > resultVertexSet;
                for (std::vector<face_type * >::iterator iter = faces.begin(); 
                        iter != faces.end(); ++iter)
                {
					std::vector<int> idx ;
					//pMesh->Face2VertexIdx(*iter, idx );
					(*iter)->Face2VertexIdx(idx);
					resultVertexSet.insert(idx[0]); 
					resultVertexSet.insert(idx[1]);
					resultVertexSet.insert(idx[2]);
                }
                this->idx_mesh2pseudo.resize(getMesh()->vertices.size());
                this->idx_pseudo2mesh.resize(resultVertexSet.size());

                unsigned count = 0;
                for (std::set<unsigned>::const_iterator iter = resultVertexSet.cbegin();
                        iter != resultVertexSet.cend(); ++iter)
                {
                        this->idx_pseudo2mesh[count] = *iter;
                        this->idx_mesh2pseudo[*iter] = count;
                        count ++ ;
                }
        }
	public :
        //The face pointers point to the original face
        std::vector<face_type * > faces; 
        std::vector<unsigned> idx_mesh2pseudo ;
        std::vector<unsigned> idx_pseudo2mesh ;
	private :
        mesh_type * m_pmesh; 
	};



};
#endif
